﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;

namespace Robotics.Tester
{
    public partial class frmMain : Form
    {
        private const Keys keyForward = Keys.S;
        private const Keys keyBackward = Keys.X;
        private const Keys keyLeft = Keys.Y;
        private const Keys keyRight = Keys.C;
        private const Keys keyHandbrake = Keys.Space;

        private bool forward = false;
        private bool backward = false;
        private bool left = false;
        private bool right = false;
        private bool handbrake = false;

        System.Threading.Thread thread = null;

        private int maxSpeed = 100;
        private int turnSpeed = 25;
        private double turnRatio = 0.5;
        private double accelerationRatio = 6.0;
        private double frictionRatio = 2.0;
        private double currentSpeed = 0;

        public frmMain()
        {
            InitializeComponent();
        }

        private void frmMain_Load(object sender, EventArgs e)
        {
            Focus();
            nxtBrick.Connect();
            thread = new Thread(MainLoop2);
            thread.Start();
        }

        public void MainLoop()
        {
            while (true)
            {
                Thread.Sleep(100);
                double speedA = 0;
                double speedB = 0;

                if (backward)
                {
                    speedA -= maxSpeed;
                    speedB -= maxSpeed;
                }
                if (forward)
                {
                    speedA += maxSpeed;
                    speedB += maxSpeed;
                }
                if (left)
                {
                    if (speedA == 0 && speedB == 0)
                    {
                        speedA -= turnSpeed;
                        speedB += turnSpeed;
                    }
                    else
                    {
                        speedA *= turnRatio;
                    }
                }
                if (right)
                {
                    if (speedA == 0 && speedB == 0)
                    {
                        speedA += turnSpeed;
                        speedB -= turnSpeed;
                    }
                    else
                    {
                        speedB *= turnRatio;
                    }

                }
                if (handbrake)
                {
                    speedA = 0;
                    speedB = 0;
                }

                nxtMotorA.Turn((int)speedA, 0);
                nxtMotorB.Turn((int)speedB, 0);
            }
        }

        private void frmMain_KeyUp(object sender, KeyEventArgs e)
        {
            switch (e.KeyCode)
            {
                case keyLeft:
                    left = false;
                    break;
                case keyRight:
                    right = false;
                    break;
                case keyForward:
                    forward = false;
                    break;
                case keyBackward:
                    backward = false;
                    break;
                case keyHandbrake:
                    handbrake = false;
                    break;
            }
        }

        public double GetConstraintedSpeed(int distance)
        {
            // FIXME
            return Math.Max(distance * 2 - 5, 0);
        }

        public void MainLoop2()
        {
            while (true)
            {
                Thread.Sleep(100);

                nxtSonar.Poll();
                int distance = nxtSonar.RawValue;
                double constraintedSpeed = GetConstraintedSpeed(distance);

                double speedA = currentSpeed;
                double speedB = currentSpeed;

                currentSpeed = Math.Sign(currentSpeed) * Math.Max(Math.Abs(currentSpeed) - frictionRatio, 0);
                speedA = speedB = currentSpeed;
                if (backward)
                {
                    currentSpeed = Math.Max(currentSpeed - accelerationRatio, -maxSpeed);
                    speedA = speedB = currentSpeed;
                }
                if (forward)
                {
                    currentSpeed = Math.Min(currentSpeed + accelerationRatio, maxSpeed);
                    speedA = speedB = currentSpeed;
                }

                if (left)
                {
                    if (speedA == 0 && speedB == 0)
                    {
                        speedA -= turnSpeed;
                        speedB += turnSpeed;
                    }
                    else
                    {
                        speedA *= turnRatio;
                    }
                }
                if (right)
                {
                    if (speedA == 0 && speedB == 0)
                    {
                        speedA += turnSpeed;
                        speedB -= turnSpeed;
                    }
                    else
                    {
                        speedB *= turnRatio;
                    }
                }

                // FIXME
                speedA = Math.Sign(speedA) * Math.Min(Math.Abs(speedA), constraintedSpeed);
                speedB = Math.Sign(speedB) * Math.Min(Math.Abs(speedB), constraintedSpeed);
                
                if (handbrake)
                {
                    speedA = speedB = currentSpeed = 0;
                }
                nxtMotorA.Turn((int)speedA, 0);
                nxtMotorB.Turn((int)speedB, 0);
            }
        }

       private void frmMain_KeyDown(object sender, KeyEventArgs e)
        {
            switch (e.KeyCode)
            {
                case keyLeft:
                    left = true;
                    break;
                case keyRight:
                    right = true;
                    break;
                case keyForward:
                    forward = true;
                    break;
                case keyBackward:
                    backward = true;
                    break;
                case keyHandbrake:
                    handbrake = true;
                    break;
            }
        }

        private void frmMain_FormClosed(object sender, FormClosedEventArgs e)
        {
            nxtMotorA.Coast();
            nxtMotorB.Coast();
            nxtBrick.Disconnect();
            thread.Abort();
        }
    }
}
